{
  "metadata": {
    "language_info": {
      "version": "3.5.2",
      "file_extension": ".py",
      "name": "python",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "pygments_lexer": "ipython3",
      "mimetype": "text/x-python",
      "nbconvert_exporter": "python"
    },
    "kernelspec": {
      "language": "python",
      "display_name": "Python 3",
      "name": "python3"
    }
  },
  "cells": [
    {
      "source": [
        "%matplotlib inline"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "\n\n********************************************************************************\n2D Robot Localization - Tutorial\n********************************************************************************\nThis tutorial introduces the main aspects of **UKF-M**.\n\nGoals of this script:\n\n- understand the main principles of Unscented Kalman Filtering on Manifolds\n  (**UKF-M**) :cite:`brossardCode2019`.\n\n- get familiar with the implementation.\n\n- design an UKF for a vanilla 2D robot localization problem.\n\n*We assume the reader to have sufficient prior knowledge with (unscented) Kalman\nfiltering. However, we require really approximate prior knowledge and intuition\nabout manifolds and tangent spaces.*\n\nThis tutorial describes all one require to design an Unscented Kalman Filter\n(UKF) on a (parallelizable) manifold, and puts in evidence the versatility and\nsimplicity of the method in term of implementation. Indeed, we need to define an\nUKF on parallelizable manifolds:\n\n1) a **model** of the state-space system that specifies the propagation and\n   measurement functions of the system.\n\n2) an **uncertainty representation** of the estimated state, which is a mapping\n   that generalizes the linear uncertainty definition $\\mathbf{e} =\n   \\mathbf{x} - \\mathbf{\\hat{x}}$.\n\n3) standard UKF parameters that are noise covariance matrices and sigma point\n   parameters.\n\nWe introduce the methodology by addressing the vanilla problem of robot\nlocalization, where the robot obtains velocity measurements, e.g., from wheel\nodometry, and position measurements, e.g., from GPS. The state consists of the\nrobot orientation along with the 2D robot position. We reproduce the example\ndescribed in :cite:`barrauInvariant2017`, Section IV.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "Import\n==============================================================================\nPackage import is minimal, as **UKF-M** is mainly build on the standard NumPy\npackage.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "import ukfm\nimport numpy as np\nimport matplotlib\nukfm.utils.set_matplotlib_config()\n#\u00a0The matplotlib configuration is adjusted for better rendering the figures."
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "The Model\n==============================================================================\nThe first ingredient we need is a **model** that defines:\n\n1) the state of the system at instant $n$, noted\n   $\\boldsymbol{\\chi}_n \\in \\mathcal{M}$, where $\\mathcal{M}$ is a\n   parallelizable manifold (vectors spaces, Lie groups and others). Here the\n   state corresponds to the robot orientation and the robot position:\n\n   .. math::\n\n      \\mathcal{M} = \\left\\{ \\begin{matrix} \\mathbf{C} \\in SO(2),\n      \\mathbf{p} \\in \\mathbb R^2 \\end{matrix} \\right\\}.\n\n2) a propagation function that describes how the state evolves along time\n\n   .. math::\n\n      \\boldsymbol{\\chi}_n = f(\\boldsymbol{\\chi}_{n-1},\n      \\boldsymbol{\\omega}_{n}, \\mathbf{w}_{n}) \\in \\mathcal{M},\n\n   where $\\boldsymbol{\\omega}_{n} \\in \\mathcal{U}$ is the input of the\n   system and $\\mathbf{w}_{n} \\sim \\mathcal{N}(\\mathbf{0},\n   \\mathbf{Q}_n)$ is a Gaussian noise.\n\n3) an observation function describing the measures we have in the form of:\n\n   .. math::\n\n      \\mathbf{y}_n = h(\\boldsymbol{\\chi}_{n}) + \\mathbf{v}_n \\in\n      \\mathbb{R}^p,\n\n   where $\\mathbf{v}_{n} \\sim \\mathcal{N}(\\mathbf{0}, \\mathbf{R}_n)$ is\n   a Gaussian noise.\n\nThe code contains models, which are declared as class. In this script, we use\nthe ``LOCALIZATION`` model.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "MODEL = ukfm.LOCALIZATION"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "<div class=\"alert alert-info\"><h4>Note</h4><p>A state or an input is an instance of the ``STATE`` or ``INPUT`` class that\n  is described in the ``MODEL``, and can thus handle a complex form not\n  restricted  to vector. In contrast, we consider for conciseness that any\n  measurement at time $n$ is a vector (1D-array).</p></div>\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "Simulating the Model\n------------------------------------------------------------------------------\nWe compute simulated data, where the robot drives along a 10 m diameter circle\nfor 40 seconds with high rate odometer measurements (100 Hz) and low rate\nposition measurements (1 Hz). We first define the model parameters, create an\ninstance of the model, and compute the true states along with noisy inputs.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "# sequence time (s)\nT = 40\n# odometry frequency (Hz)\nodo_freq = 100\n# \u00a0create the model\nmodel = MODEL(T, odo_freq)\n# odometry noise standard deviation\nodo_std = np.array([0.01,          # longitudinal speed (v/m)\n                    0.01,          # transverse shift speed (v/m)\n                    1/180*np.pi])  # differential odometry (rad/s)\n# radius of the circle trajectory (m)\nradius = 5\n# simulate trajectory\nstates, omegas = model.simu_f(odo_std, radius)"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "<div class=\"alert alert-info\"><h4>Note</h4><p>The model encodes how non-linear noise affects the propagation function. In\n  contrast, we assume measurement noise affects the observations linearly. It\n  spares us computational time, but the method can be adapted to handle\n  non-linear observation noises of the form\n  $\\mathbf{y}_n = h(\\boldsymbol{\\chi}_{n}, \\mathbf{v}_n)$.</p></div>\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "The state and input variables are both a list of ``STATE`` and ``INPUT``\ninstances. One can access a variable at specific instant $n$ as:\n\n.. highlight:: python\n.. code-block:: python\n\n    state_n = states[n] # model.STATE instance\n    omega_n = omegas[n] #\u00a0model.INPUT instance\n\nWe can access to the elements of the state or the input as:\n\n.. highlight:: python\n.. code-block:: python\n\n    state_n.Rot    # 2d orientation encoded in a rotation matrix\n    state_n.p      # 2d position\n    omega_n.v      # robot forward velocity\n    omega_n.gyro   # robot angular velocity\n\nThese elements depend on the considered problem and are defined in the model.\nSee at the :meth:`~ukfm.LOCALIZATION`  documentation to get a brief\nmathematical description.\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>The orientation ``states[n].Rot`` is defined via a rotation matrix. We\n  always define for clarity orientations in matrices living  in $SO(2)$\n  and $SO(3)$. The method remains compatible with quaternion. We may\n  drop some numerical issues, e.g. round-off that leads to non-orthogonal\n  rotation matrices, to let the code simple.</p></div>\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "With the *true* states, we simulate *noisy* measurements.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "# GPS frequency (Hz)\ngps_freq = 1\n# GPS noise standard deviation (m)\ngps_std = 1\n# simulate measurements\nys, one_hot_ys = model.simu_h(states, gps_freq, gps_std)"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "The variable ``ys`` is a 2D array that contains all the observations of the\nsequence. To get the k-th measurement, take the k-th element of the variable\nas:\n\n.. highlight:: python\n.. code-block:: python\n\n      y_k = ys[k] # vector (1D array)\n\nWe have defined an array ``one_hot_ys`` that contains 1 at instant where a\nmeasurement happens and 0 otherwise.\n\nLet us visualizes the robot trajectory along with measurements.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "model.plot_traj(states, ys)"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "GPS measurements are visibly noisy.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "Filter Design\n==============================================================================\nDesigning an UKF on parallelizable manifolds consists in:\n\n1) defining a model of the propagation function and the measurement function.\n\n2) choosing the retraction $\\varphi(.,.)$ and inverse retraction\n   $\\varphi^{-1}_.(.)$ such that\n\n   .. math::\n\n     \\boldsymbol{\\chi} &= \\varphi(\\hat{\\boldsymbol{\\chi}}, \\boldsymbol{\\xi})\n     \\in \\mathcal{M},\n\n     \\boldsymbol{\\xi} &= \\varphi^{-1}_{\\hat{\\boldsymbol{\\chi}}}\n     (\\boldsymbol{\\chi}) \\in \\mathbb{R}^d,\n\n   where $\\boldsymbol{\\chi}$ is the true state,\n   $\\hat{\\boldsymbol{\\chi}}$ the estimated state, and\n   $\\boldsymbol{\\xi}$ the state uncertainty (we does not use the\n   notation $\\mathbf{x}$ and $\\mathbf{e}$ to emphasize the\n   differences with the linear case).\n\n3) setting UKF parameters such as sigma point dispersion and noise covariance\n   values.\n\nStep 1) is already done, as we take the functions defined in the model.\n\nStep 2) consists in choosing the mapping that encode our representation of the\nstate belief. A basic UKF is building on the uncertainty defined as\n$\\mathbf{e} = \\mathbf{x} - \\mathbf{\\hat{x}}$, which is not necessary\noptimal. Rather than, we define the uncertainty $\\boldsymbol{\\xi}$\nthought $\\boldsymbol{\\chi} = \\varphi(\\hat{\\boldsymbol{\\chi}},\n\\boldsymbol{\\xi})$, where the *retraction* $\\varphi(.,.)$ has to\nsatisfy $\\varphi(\\boldsymbol{\\chi}, \\mathbf{0}) = \\boldsymbol{\\chi}$\n(without uncertainty, the estimated state equals the true state). We then need\nan *inverse retraction* to get a difference from two states that must respect\n$\\varphi^{-1}_{\\boldsymbol{\\chi}}(\\boldsymbol{\\chi}) = \\mathbf{0}$.\n\nWe embed here the state in $SO(2) \\times \\mathbb{R}^2$, such that:\n\n* the retraction $\\varphi(.,.)$ is the $SO(2)$ exponential for\n  orientation and the vector addition for position.\n\n* the inverse retraction $\\varphi^{-1}_.(.)$ is the $SO(2)$\n  logarithm for orientation and the vector subtraction for position.\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>We define different choices of retraction and inverse retraction\n    directly in the model.</p></div>\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>One can suggest alternative retractions, e.g. by viewing the state as a\n  element of $SE(2)$. In the benchmarks section, we compare different\n  choices of retraction for different problems.</p></div>\n\nWe define the filter parameters based on the model.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "# propagation noise covariance matrix\nQ = np.diag(odo_std ** 2)\n# measurement noise covariance matrix\nR = gps_std ** 2 * np.eye(2)\n# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3])\n# this parameter scales the sigma points.\n# Current values are between 10^-3 and 1."
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "Filter Initialization\n------------------------------------------------------------------------------\nWe initialize the filter with the true state with a small heading error of 1\u00b0.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "# \"add\" orientation error to the initial state\nstate0 = model.STATE(Rot=states[0].Rot.dot(ukfm.SO2.exp(1/180*np.pi)),\n                     p=states[0].p)\n# initial state uncertainty covariance matrix\nP0 = np.zeros((3, 3))\n# The state is not perfectly initialized\nP0[0, 0] = (1/180*np.pi)**2"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "We define the filter as an instance  of the ``UKF`` class.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "ukf = ukfm.UKF(state0=state0,          # \u00a0initial state\n               P0=P0,                  # initial covariance\n               f=model.f,              # propagation model\n               h=model.h,              # observation model\n               Q=Q,                    # process noise covariance\n               R=R,                    # observation noise covariance\n               phi=model.phi,          # retraction\n               phi_inv=model.phi_inv,  # inverse retraction\n               alpha=alpha             # sigma point parameters\n               )"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "Before launching the filter, we set a list for recording estimates along the\nfull trajectory and a 3D array to record covariance estimates.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "ukf_states = [ukf.state]\nukf_Ps = np.zeros((model.N, 3, 3))\nukf_Ps[0] = ukf.P"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "Filtering\n------------------------------------------------------------------------------\nThe UKF proceeds as a standard Kalman filter with a for loop.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "# measurement iteration number (first measurement is for n == 0)\nk = 1\nfor n in range(1, model.N):\n    # propagation\n    ukf.propagation(omegas[n-1], model.dt)\n    # update only if a measurement is received\n    if one_hot_ys[n] == 1:\n        ukf.update(ys[k])\n        k += 1\n    # save estimates\n    ukf_states.append(ukf.state)\n    ukf_Ps[n] = ukf.P"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "Results\n==============================================================================\nWe plot the trajectory, GPS measurements and estimated trajectory. We then\nplot the orientation and position errors along with 95% ($3\\sigma$)\nconfident interval. The results has to be confirmed with average metrics to\nreveal the filter performances in term of accuracy, consistency and\nrobustness.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "model.plot_results(ukf_states, ukf_Ps, states, ys)"
      ],
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "All results seem coherent. This is expected as the initial heading error is\nsmall.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    },
    {
      "source": [
        "Conclusion\n==============================================================================\nThis script introduces UKF-M and shows how designing an UKF on parallelizable\nmanifolds mainly consists in choosing an advantageous uncertainty\nrepresentation. Two major interests of the method are that many problems could\nbe addressed within the framework, and that both the theory and its\nimplementation are sufficiently simple.\n\nThe filter works apparently well on a simple robot localization problem, with\nsmall initial heading error. Is it hold for more challenging initial error ?\n\nYou can now:\n\n* enter more in depth with the theory, see :cite:`brossardCode2019`.\n\n* address the UKF for the same problem with different noise parameters, and\n  test its robustness to strong initial heading error.\n\n* modify the propagation model with a differential odometry model, where\n  inputs are left and right wheel speed measurements.\n\n* apply the UKF for the same problem on real data.\n\n* benchmark the UKF with different retractions and compare the new filters\n  to both the extended Kalman filter and invariant extended Kalman filter of\n  :cite:`barrauInvariant2017`.\n\n"
      ],
      "metadata": {},
      "cell_type": "markdown"
    }
  ],
  "nbformat": 4,
  "nbformat_minor": 0
}